/*
 * DSPTools.c
 *
 * Created: 28/01/2014 16:40:56
 *  Author: edielsonpf
 */ 

#include "stdlib.h"
#include "math.h"
#include "DSPTools.h"

typedef struct tag_DSPTools
{
	double	dLastPreData;
	double* pdHamming;
} ttag_DSPTools;


/*********************************************************
    Private operations
*********************************************************/
/*
	Create instance. 
	@param	void
			
	@return Handle - the instance class handle 
	
	@author edielsonpf
	@date   18/02/2014
*/
Handle DSPTools_new(void)
{
	ttag_DSPTools*	ptagDSPTools = (ttag_DSPTools*)malloc(sizeof(ttag_DSPTools));
	if(ptagDSPTools)
	{
		ptagDSPTools->dLastPreData=0;
		ptagDSPTools->pdHamming = NULL;
	}
	return (Handle)ptagDSPTools;
}

/*
	Delete instance. 
	@param	Handle hInstance - The instance handle for deletion
		
	@return 
	
	@author edielsonpf
	@date   18/02/2014
*/
Bool8 DSPTools_delete(Handle hHandle)
{
	if(!hHandle)	return False8;
	ttag_DSPTools*	ptagDSPTools = (ttag_DSPTools*)hHandle;
	if(ptagDSPTools->pdHamming) free(ptagDSPTools->pdHamming);
	free(ptagDSPTools);
	return True8;
}
/*
 	Calculates the pre-emphasis of the input signal
	
	@param	double_t* dpInputData - Input vector with data for coefficients extraction
	@param	long lInputDataSize - Input vector size
	
	@return 
 	
	Created: 28/01/2014 16:31:15
	Author: edielsonpf
 */ 
tenu_DSPResult DSPTools_calcPreEmphasis(Handle hHandle, double_t* dpInputData, long lInputDataSize)
{
	if(!dpInputData)	return DSP_INVALID_PARAMAMETER;
	if(!hHandle)		return DSP_INVALID_PARAMAMETER;
	
	ttag_DSPTools*	ptagDSPTools = (ttag_DSPTools*)hHandle;
	long lCounter; 
		
	dpInputData[0]=dpInputData[0]-0.95*ptagDSPTools->dLastPreData;
	for (lCounter=1;lCounter<lInputDataSize;lCounter++) dpInputData[lCounter]=dpInputData[lCounter]-0.95*dpInputData[lCounter-1];
	ptagDSPTools->dLastPreData=dpInputData[lCounter-1];
	return DSP_SUCCESS;
}

/*
 	Calculates the pre-emphasis of the input signal
	
	@param	double_t* dpInputData - Input vector with data for coefficients extraction
	@param	long lInputDataSize - Input vector size
	
	@return 
 	
	Created: 28/01/2014 16:31:15
	Author: edielsonpf
 */ 
tenu_DSPResult DSPTools_resetPreEmphasis(Handle hHandle)
{
	if(!hHandle)		return DSP_INVALID_PARAMAMETER;
	ttag_DSPTools*	ptagDSPTools = (ttag_DSPTools*)hHandle;
	ptagDSPTools->dLastPreData=0;
	return DSP_SUCCESS;
}
/*
 	Creates a Hamming window
	
	@param	long lHammingSize - The Hamming window size
	
	@return double_t* - pointer to the Hamming window
 	
	Created: 28/01/2014 16:31:15
	Author: edielsonpf
 */ 
double_t* DSPTools_createHamming(Handle hHandle, long lHammingSize)
{
	if(!hHandle)	return NULL;
	ttag_DSPTools*	ptagDSPTools = (ttag_DSPTools*)hHandle;
	long lCounter;
	
	if(ptagDSPTools->pdHamming) free(ptagDSPTools->pdHamming);
	ptagDSPTools->pdHamming = (double_t*) malloc (sizeof(double_t) * (lHammingSize));
	if(ptagDSPTools->pdHamming)
	{
		for(lCounter=0;lCounter<lHammingSize;lCounter++)
		{
			ptagDSPTools->pdHamming[lCounter] = 0.54 - 0.46*cos((2*M_PI*lCounter)/(lHammingSize-1));
		}	
	}
	return ptagDSPTools->pdHamming;
}

/*
 	Deletes the Hamming window
	
	@param	double_t* pdHamming - Pointer to the Hamming window
	
	@return tenu_DSPResult
 	
	Created: 28/01/2014 16:31:15
	Author: edielsonpf
 */ 
tenu_DSPResult DSPTools_deleteHamming(Handle hHandle)
{
	if(!hHandle)	return DSP_INVALID_PARAMAMETER;
	ttag_DSPTools*	ptagDSPTools = (ttag_DSPTools*)hHandle;
	
	if(ptagDSPTools->pdHamming) free(ptagDSPTools->pdHamming);
	return DSP_SUCCESS;
}

/*
 	Calculates the FFT
	
	@param	double_t* pdInputData, 
	@param	double_t* pdOutputData
	@param	int M 
	@param	int iFFTSize
	@param	int iWindowSize
	
	@return void
 	
	Created: 28/01/2014 16:31:15
	Author: edielsonpf
 */ 
tenu_DSPResult DSPTools_fft(_IN_ double_t* pdInputData,_IN_ int iWindowSize, _OUT_ double_t* pdOutputData, _IN_ int iFFTSize, _IN_ int M)
{
	if(!pdInputData) return DSP_INVALID_PARAMAMETER;
	if(!pdOutputData) return DSP_INVALID_PARAMAMETER;
	
	double Tr, Ti, Ur, Ui, Wr, Wi, b;   // var auxiliares para o calculo da FFT
  	int ip, n1, n2, n3;                 // var aux para o calculo da FFT
  	register int i, j, k;               // var auxiliares para loops
	double *Xr,*Xi;     				// partes real e imaginaria da FFT
  
	Xr = (double*) malloc (sizeof(double) * (iFFTSize));
	if(!Xr) return DSP_ERROR;

	Xi = (double *) malloc (sizeof(double) * (iFFTSize));			
	if(!Xi) return DSP_ERROR;
	
	Tr = Ti = Ur = Ui = Wr = Wi = b = 0.0;
  	ip = n1 = n2 = n3 = 0;
	
  	// Calculo da FFT de N pontos do vetor pdInputData. Usa-se o algoritmo proposto no
  	// Prob 6.4, pagina 333, do livro "Digital Signal Processing" de Oppenheim &
  	// Schaffer. Nomenclatura usada no livro e aqui: LE= n1, LE1= n2, NV2= n3.
  	/*--------------------------------------------------------------*/
	for(k=0; k<iFFTSize; k++) // inicializacao
  	{
		Xr[k] = 0.0;    
		Xi[k] = 0.0;
  	}
  	for(k=0; k<iWindowSize; k++) Xr[k] = pdInputData[k];

  	// Embaralhamento da sequencia de entrada
  	n3 = iFFTSize / 2;
  	j = 0;
  	for(i=0; i<iFFTSize-1; i++) // comeco do "for"
  	{
    	if(i < j)
    	{
	  		Tr = Xr[j];
      		Xr[j] = Xr[i];
      		Xr[i] = Tr;
   		/* Ti = Xi[j];      // para seq.complexas
      		Xi[j] = Xi[i];
      		Xi[i] = Ti;  */
		}
		k = n3;
		while(k <= j)
		{
			j -= k;
			k /= 2;
		}
		j += k;
  	}     // fim do "for" (e do embaralhamento)
	/*--------------------------------------------------------------*/
	
  	//-------------------------------
  	// Calculo da FFT
  	//-------------------------------
	/*--------------------------------------------------------------*/
	for(k=1; k<=M; k++) // "for 1"
  	{
    	n1 = 1;
		for(i=1; i<=k; i++) n1*= 2;// equivale a fazer 2**k
		n2 = n1 / 2;
		Ur = 1.0;
    	Ui = 0.0;
		b = M_PI / n2;                  // M_PI=3.1415... (definido em <math.h>)
		Wr = cos(b);
    	Wi = -sin(b);

		for(j=0; j<n2; j++) // "for 2"
		{
	  		for(i=j; i<iFFTSize; i+=n1) // "for 3"
			{
				ip = i + n2;
				Tr = Xr[ip] * Ur - Xi[ip] * Ui;
				Ti = Xr[ip] * Ui + Xi[ip] * Ur;
				Xr[ip] = Xr[i] - Tr;
				Xi[ip] = Xi[i] - Ti;
				Xr[i] =  Xr[i] + Tr;
				Xi[i]  = Xi[i] + Ti;
			}     // fim "for 3"
	  		Tr = Ur;
	  		Ur = Ur * Wr - Ui * Wi;
      		Ui = Tr * Wi + Ui * Wr;
		}        // fim "for 2"
  	}           // fim "for 1"
  	// Fim do calculo da FFT. A seq. obtida esta na ordem sequencial
	/*--------------------------------------------------------------*/
	

  	// Calculo do quadrado do modulo da FFT (equivale a energia)
	/*--------------------------------------------------------------*/
	for(k=0; k<iFFTSize/2; k++)
		pdOutputData[k] = Xr[k]*Xr[k] + Xi[k]*Xi[k];
//	for(k=0; k<fft_SIZE; k++)
//		pdOutputData[k] = sqrt(Xr[k] * Xr[k] + Xi[k] * Xi[k]);
  	/*--------------------------------------------------------------*/

  	// Desalocando ponteiros
  	free(Xr);
  	free(Xi);
	  
	return DSP_SUCCESS;  
}       // fim da funcao "fft()"